
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_plane.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <rtthread.h>

#include <rc_channel/rc_channel.h>
#include <common/microbee.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void init_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos ch_flag);
static bool do_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos ch_flag);
static void mode_switch_changed(rc_channel_t rc, modeswitch_pos_t new_pos);

static bool has_valid_input(rcs_channel_t rcs);
static rc_channel_t get_arming_channel(rcs_channel_t rcs);
static rc_channel_t channel(rcs_channel_t rcs, const uint8_t chan);
static int8_t flight_mode_channel_number(rcs_channel_t rcs);
static void read_mode_switch(rcs_channel_t rcs);
/*----------------------------------variable----------------------------------*/
struct rc_channel obj_channels[NUM_RC_CHANNELS];

static struct rc_ops rc_channel_ops = {.init_aux_function   = init_aux_function,
                                       .do_aux_function     = do_aux_function,
                                       .mode_switch_changed = mode_switch_changed};

static struct rcs_ops rc_channels_ops = {.channel = channel,
                                         .read_mode_switch = read_mode_switch,
                                         .has_valid_input = has_valid_input,
                                         .get_arming_channel = get_arming_channel,
                                         .arming_check_throttle = NULL,
                                         .flight_mode_channel_number = flight_mode_channel_number};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void rc_do_aux_function_change_mode(rc_channel_t rc, const enum ModeNumber number, const enum RcAuxSwitchPos ch_flag)
{
    switch(ch_flag) {
    case RC_AUX_SWITCH_HIGH: {
        // engage mode (if not possible we remain in current flight mode)
        fms_set_mode_by_number(number, MODE_REASON_RC_COMMAND);
        break;
    }
    default:
        // return to flight mode switch's flight mode if we are currently
        // in this mode
        if (mode_number(fms.control_mode) == number) {
            rc_reset_mode_switch(rc);
        }
    }
}

static void init_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos ch_flag)
{
    switch(ch_option) {
    // the following functions do not need to be initialised:
    case RC_AUX_AUTO:
    case RC_AUX_CIRCLE:
    case RC_AUX_ACRO:
    case RC_AUX_TRAINING:
    case RC_AUX_FLAP:
    case RC_AUX_GUIDED:
    case RC_AUX_INVERTED:
    case RC_AUX_LOITER:
    case RC_AUX_MANUAL:
    case RC_AUX_RTL:
    case RC_AUX_TAKEOFF:
    case RC_AUX_FBWA:
    case RC_AUX_AIRBRAKE:
#if HAL_QUADPLANE_ENABLED
    case RC_AUX_QRTL:
    case RC_AUX_QSTABILIZE:
#endif
    case RC_AUX_FBWA_TAILDRAGGER:
    case RC_AUX_FWD_THR:
    case RC_AUX_LANDING_FLARE:
    case RC_AUX_PARACHUTE_RELEASE:
    case RC_AUX_MODE_SWITCH_RESET:
    case RC_AUX_CRUISE:
#if HAL_QUADPLANE_ENABLED
    case RC_AUX_ARMDISARM_AIRMODE:
#endif
    case RC_AUX_PLANE_AUTO_LANDING_ABORT:
    case RC_AUX_TRIM_TO_CURRENT_SERVO_RC:
    case RC_AUX_EMERGENCY_LANDING_EN:
    case RC_AUX_FW_AUTOTUNE:
        break;

    case RC_AUX_SOARING:
#if HAL_QUADPLANE_ENABLED
    case RC_AUX_Q_ASSIST:
    case RC_AUX_AIRMODE:
    case RC_AUX_WEATHER_VANE_ENABLE:
#endif
#if AP_AIRSPEED_AUTOCAL_ENABLE
    case RC_AUX_ARSPD_CALIBRATE:
#endif
    case RC_AUX_TER_DISABLE:
    case RC_AUX_CROW_SELECT:
        rc_run_aux_function(rc, ch_option, ch_flag, RC_FUNC_TRIGGER_SOURCE_INIT);
        break;

    case RC_AUX_REVERSE_THROTTLE:
        fms.have_reverse_throttle_rc_option = true;
        // setup input throttle as a range. This is needed as init_aux_function is called
        // after set_control_channels()
        if (fms.channel_throttle) {
            rc_set_range(fms.channel_throttle, 100);
        }
        // note that we don't call do_aux_function() here as we don't
        // want to startup with reverse thrust
        break;

    default:
        // handle in parent class
        rc_InitAuxFunction(rc, ch_option, ch_flag);
        break;
    }
}

// do_aux_function - implement the function invoked by auxiliary switches
static bool do_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos ch_flag)
{
    switch(ch_option) {
    case RC_AUX_INVERTED:
        fms.inverted_flight = (ch_flag == RC_AUX_SWITCH_HIGH);
        break;

    case RC_AUX_REVERSE_THROTTLE:
        fms.reversed_throttle = (ch_flag == RC_AUX_SWITCH_HIGH);
        gcs_send_text(MAV_SEVERITY_INFO, "RevThrottle: %s", fms.reversed_throttle?"ENABLE":"DISABLE");
        break;

    case RC_AUX_MANUAL:
        rc_do_aux_function_change_mode(rc, MN_MANUAL, ch_flag);
        break;

    case RC_AUX_FLAP:
    case RC_AUX_FBWA_TAILDRAGGER:
    case RC_AUX_AIRBRAKE:
        break; // input labels, nothing to do

    case RC_AUX_FWD_THR:
        break; // VTOL forward throttle input label, nothing to do

    default:
        return rc_DoAuxFunction(rc, ch_option, ch_flag);
    }

    return true;
}

static void mode_switch_changed(rc_channel_t rc, modeswitch_pos_t new_pos)
{
    if (new_pos < 0 || (uint8_t)new_pos > fms.num_flight_modes) {
        // should not have been called
        return;
    }

    fms_set_mode_by_number((enum ModeNumber)fms.flight_modes[new_pos], MODE_REASON_RC_COMMAND);
}

/// rc_channels
static bool has_valid_input(rcs_channel_t rcs)
{
    (void)rcs;

    if (fms_rc_failsafe_active() || fms.failsafe.rc_failsafe) {
        return false;
    }
    if (fms.failsafe.throttle_counter != 0) {
        return false;
    }
    return true;
}

static rc_channel_t get_arming_channel(rcs_channel_t rcs)
{
    return fms.channel_rudder;
}

static rc_channel_t channel(rcs_channel_t rcs, const uint8_t chan)
{
    if (chan >= NUM_RC_CHANNELS) {
        return NULL;
    }
    return &obj_channels[chan];
}

static int8_t flight_mode_channel_number(rcs_channel_t rcs)
{
    return fms.g.flight_mode_channel;
}

static void read_mode_switch(rcs_channel_t rcs)
{
    (void)rcs;

    if (time_millis() - fms.failsafe.last_valid_rc_ms > 100) {
        // only use signals that are less than 0.1s old.
        return;
    }
    rcs_ReadModeSwitch();
}

// 
static int rcs_plane_ctor(void)
{
    rcs_ctor(&rc_channels_ops);

    // setup ch_in on channels
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rc_ctor(&obj_channels[i], &rc_channel_ops);
    }

    return 0;
}
MB_INIT_COMPONENT_EXPORT(rcs_plane_ctor);
/*------------------------------------test------------------------------------*/


